#include "Rectangle.h"

namespace coolpool
{
	Rectangle::Rectangle(const Rectangle & rectangle) :
		RigidBody(rectangle)
	{
		bounding_cylinder_ = rectangle.bounding_cylinder_;
		plane_ = rectangle.plane_;
		texture_ = rectangle.texture_;
		v1_ = rectangle.v1_;
		v2_ = rectangle.v2_;
		v3_ = rectangle.v3_;
		v4_ = rectangle.v4_;
	}

	Rectangle::Rectangle() :
		RigidBody()
	{
		v1_ = Vector3D(1, 0, 0);
		v2_ = Vector3D(0, 0, 0);
		v3_ = Vector3D(0, 0, 1);
		v4_ = Vector3D(1, 0, 1);
		init();
	}

	// TODO: The normal to the plane is added by right hand rule, the points must be added clockwise.
	Rectangle::Rectangle(const Vector3D & v1, const Vector3D & v2, const Vector3D & v3, const Vector3D & v4) :
		RigidBody()
	{
		v1_ = v1;
		v2_ = v2;
		v3_ = v3;
		v4_ = v4;
		init();
	}

	Rectangle::~Rectangle()
	{
	}

	// Reordering of the vertices and the two sides, then
	// init of the containing plane and the bounding sphere.
	void Rectangle::init()
	{
		if ((v1_ - v2_).dot(v3_ - v2_) != 0)
			swap(v1_, v2_); // Now they must be ordered if they create a valid rectangle.

		Vector3D side1 = v1_ - v2_;
		Vector3D side2 = v3_ - v2_;
		// Perpendicularity check
		if (side1.dot(side2) != 0 || (v2_ - v3_).dot(v4_ - v3_) != 0)
			throw NoSuchRectangleException();

		Vector3D normal = side1 * side2; // Cross product
		plane_ = math_tools::Plane(v1_, normal);

		// Setup bounding cylinder.
		Eigen::Vector2d v1e;
		v1e[0] = v1_.getX();
		v1e[1] = v1_.getZ();
		Eigen::Vector2d v3e;
		v3e[0] = v3_.getX();
		v3e[1] = v3_.getZ();
		Eigen::Vector2d half_diagonal = (v3e - v1e) * (0.5);
        double radius = sqrt(math_tools::sqr(half_diagonal[0])+math_tools::sqr(half_diagonal[1]));
		Eigen::Vector2d center = v1e + half_diagonal;
		bounding_cylinder_ = BoundingCylinder(center, radius);
	}

	bool Rectangle::isStatic() const
	{
		return true;
	}

	bool Rectangle::isInRec(const Vector3D & point)
	{
		// Projection onto plane.
		Vector3D projection = plane_.closestPoint(point);

		// Volume of the rectangle
		Vector3D side1 = v1_ - v2_;
		Vector3D side2 = v3_ - v2_;

		Vector3D con = projection - v2_;
		if (side1.dot(con) < 0 || side2.dot(con) < 0)
			return false;

		// Projection on the edges of the rectangle - squared for optimalization purposes
		float t1p = math_tools::sqr(side1.dot(projection - v2_)) / side1.magSqr();
		float t2p = math_tools::sqr(side2.dot(projection - v2_)) / side2.magSqr();

		return !(t1p > side1.magSqr() || t2p > side2.magSqr());
	}

	Entity * Rectangle::clone() const
	{
		return new Rectangle(*this);
	}

	const BoundingCylinder & Rectangle::getBoundingCylinder()
	{
		return bounding_cylinder_;
	}

	IRenderable * Rectangle::getRenderable()
	{
		return new RectangleRenderable(v1_, v2_, v3_, v4_, texture_);
	}

	int Rectangle::getType() const
	{
		return CP_REC;
	}

	void Rectangle::onUpdate(double time_step)
	{

	}

	Vector3D Rectangle::closestPoint(const Vector3D & point)
	{
		return plane_.closestPoint(point);
	}

	double Rectangle::distanceFrom(const Vector3D & point)
	{
		return plane_.distanceFrom(point);
	}

	double Rectangle::distanceFromSqr(const Vector3D & point)
	{
		return plane_.distanceFromSqr(point);
	}
}
